
#include "ai_vehicle.h"

using namespace WtfEngine;
using namespace WtfEngine::Behaviours;

#define STEERING_SLOP	0.04

bool VehicleMotionControl::Evaluate(const IMobileActor::tRef& rActor, const tDirection& vDesiredVelocity) {
	Vehicle * pActor = dynamic_cast<Vehicle *>(&*rActor);
	if(pActor) {
		tDirection vForward(rActor->getOrientation() * tVector(0, 0, -1));
		tDirection vCurrentVel(pActor->getVelocity());
		tDirection vDesiredVelNorm(vDesiredVelocity.normalize());

		// Get vector pointing left (orthogonal to current direction and up vector)
		tDirection vLeft(vForward.cross(tVector(0, -1, 0)).normalize());
		
		tScalar fSteering = vLeft.dot(vDesiredVelNorm) / 2;
		
		if(vForward.dot(vDesiredVelNorm) < 0) {
			// Need to travel backwards
			if(vForward.dot(vCurrentVel) > 0) {
				// Currently travelling forwards, apply brake
				pActor->setThrottle(0);
				pActor->setBrake(0.5);
			} else {
				// Reverse
				pActor->setThrottle(-1.0);
				pActor->setBrake(0);
			};
		} else if(vCurrentVel.length() < vDesiredVelocity.length()) {
			pActor->setThrottle(1.0);
			pActor->setBrake(0);
		} else if(vCurrentVel.length() > vDesiredVelocity.length()) {
			pActor->setThrottle(0);
			pActor->setBrake(0.1);
		} else {
			pActor->setThrottle(0);
			pActor->setBrake(0);
		};

		// Perform steering
		if(abs(fSteering) > STEERING_SLOP)
			pActor->setSteering(fSteering);

		return true;
	} else {
		throw new std::logic_error("Trying to apply vehicle motion control behaviour to non-vehicle");
	};
};

